
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mavproxy_common.c
  * @author     baiyang
  * @date       2021-8-25
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mavproxy_monitor.h"

#include "mavproxy.h"
#include "mavcmd.h"

#include <c_library_v2/common/mavlink.h>
#include <c_library_v2/ardupilotmega/mavlink.h>

#include <copter/fms.h>
#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static const float magic_force_arm_value = 2989.0f;
static const float magic_force_disarm_value = 21196.0f;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mavproxy_rc_manual_override(RC_HandleTypeDef *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed)
{
    if (c == NULL) {
        return;
    }
    int16_t override_value = 0;
    if (value_in != INT16_MAX) {
        const int16_t radio_min = c->_radio_min;
        const int16_t radio_max = c->_radio_max;
        if (reversed) {
            value_in *= -1;
        }
        override_value = radio_min + (radio_max - radio_min) * (value_in + offset) / scaler;
    }
    RC_set_override(c, override_value, tnow);
}

// allow override of RC channel values for complete GCS
// control of switch position and RC PWM values.
void mavproxy_handle_rc_channels_override(const mavlink_message_t *msg)
{
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    if(msg->sysid != sysid_this_mav.sysid) {
        return; // Only accept control from our gcs
    }

    const uint32_t tnow = time_millis();

    mavlink_rc_channels_override_t packet;
    mavlink_msg_rc_channels_override_decode(msg, &packet);

    const uint16_t override_data[] = {
        packet.chan1_raw,
        packet.chan2_raw,
        packet.chan3_raw,
        packet.chan4_raw,
        packet.chan5_raw,
        packet.chan6_raw,
        packet.chan7_raw,
        packet.chan8_raw,
        packet.chan9_raw,
        packet.chan10_raw,
        packet.chan11_raw,
        packet.chan12_raw,
        packet.chan13_raw,
        packet.chan14_raw,
        packet.chan15_raw,
        packet.chan16_raw
    };

    for (uint8_t i=0; i<8; i++) {
        // Per MAVLink spec a value of UINT16_MAX means to ignore this field.
        if (override_data[i] != UINT16_MAX) {
            RC_set_override(&(fms.channels.channel[i]), override_data[i], tnow);
        }
    }
    for (uint8_t i=8; i<ARRAY_SIZE(override_data); i++) {
        // Per MAVLink spec a value of zero or UINT16_MAX means to
        // ignore this field.
        if (override_data[i] != 0 && override_data[i] != UINT16_MAX) {
            // per the mavlink spec, a value of UINT16_MAX-1 means
            // return the field to RC radio values:
            const uint16_t value = override_data[i] == (UINT16_MAX-1) ? 0 : override_data[i];
            RC_set_override(&(fms.channels.channel[i]), value, tnow);
        }
    }

    //gcs().sysid_myggcs_seen(tnow);

}

MAV_RESULT mavproxy_handle_command_component_arm_disarm(const mavlink_command_long_t *packet)
{
    if (math_flt_equal(packet->param1,1.0f)) {
        if (arming_is_armed(&fms.arming)) {
            return MAV_RESULT_ACCEPTED;
        }
        // run pre_arm_checks and arm_checks and display failures
        const bool do_arming_checks = !math_flt_equal(packet->param2,magic_force_arm_value);
        if (fms.arming.ops->arm(&fms.arming, ARMING_CHECK_MAVLINK, do_arming_checks)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }
    if (math_flt_zero(packet->param1))  {
        if (!arming_is_armed(&fms.arming)) {
            return MAV_RESULT_ACCEPTED;
        }
        const bool forced = math_flt_equal(packet->param2, magic_force_disarm_value);
        // note disarm()'s second parameter is "do_disarm_checks"
        if (fms.arming.ops->disarm(&fms.arming, ARMING_CHECK_MAVLINK, !forced)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }

    return MAV_RESULT_UNSUPPORTED;
}

MAV_RESULT mavproxy_handle_command_do_set_mode(const mavlink_command_long_t *packet)
{
    const MAV_MODE _base_mode = (MAV_MODE)packet->param1;
    const uint32_t _custom_mode = (uint32_t)packet->param2;

    return mavproxy_set_mode_common(_base_mode, _custom_mode);
}

static void mavproxy_send_command_ack(uint16_t command, uint8_t result, 
    mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_msg_command_ack_encode(sysid_this_mav.sysid, sysid_this_mav.compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

static void mavproxy_send_command_ack_sysid(uint16_t command, uint8_t result,
    mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};

    mavlink_msg_command_ack_encode(msg->sysid, msg->compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

void mavproxy_proc_command(mavlink_command_long_t* command,
    mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_PREFLIGHT_CALIBRATION: {
        if (command->param1 == 1) { // calibration gyr
            mavcmd_set(MAVCMD_CALIBRATION_GYR, NULL);
        } else if (command->param2 == 1) { // calibration mag
            mavcmd_set(MAVCMD_CALIBRATION_MAG, NULL);
        } else if (command->param5 == 1) { // calibration acc
            mavcmd_set(MAVCMD_CALIBRATION_ACC, NULL);
        } else if (command->param5 == 2) { // calibration level
            // mavproxy_send_statustext_msg(CAL_START_LEVEL, msg);
        } else {
            /* all 0 command, cancel current process */
        }

        result = MAV_CMD_ACK_OK | MAV_CMD_ACK_ENUM_END;
        break;
    }

    case MAV_CMD_COMPONENT_ARM_DISARM:
        result = mavproxy_handle_command_component_arm_disarm(command);
        break;

    case MAV_CMD_DO_SET_MODE:
        result = mavproxy_handle_command_do_set_mode(command);
        break;

    default:
        break;
    }

    mavproxy_send_command_ack(command->command, result, msg);
}

// return MAV_TYPE corresponding to frame class
MAV_TYPE mavproxy_get_frame_mav_type()
{
    switch ((motor_frame_class)PARAM_GET_INT8(VEHICLE,FRAME_CLASS)) {
        case MOTOR_FRAME_QUAD:
        case MOTOR_FRAME_UNDEFINED:
            return MAV_TYPE_QUADROTOR;
        case MOTOR_FRAME_HEXA:
        case MOTOR_FRAME_Y6:
            return MAV_TYPE_HEXAROTOR;
        case MOTOR_FRAME_OCTA:
        case MOTOR_FRAME_OCTAQUAD:
            return MAV_TYPE_OCTOROTOR;
        case MOTOR_FRAME_HELI:
        case MOTOR_FRAME_HELI_DUAL:
        case MOTOR_FRAME_HELI_QUAD:
            return MAV_TYPE_HELICOPTER;
        case MOTOR_FRAME_TRI:
            return MAV_TYPE_TRICOPTER;
        case MOTOR_FRAME_SINGLE:
        case MOTOR_FRAME_COAX:
        case MOTOR_FRAME_TAILSITTER:
            return MAV_TYPE_COAXIAL;
        case MOTOR_FRAME_DODECAHEXA:
            return MAV_TYPE_DODECAROTOR;
        case MOTOR_FRAME_DECA:
            return MAV_TYPE_DECAROTOR;
    }
    // unknown frame so return generic
    return MAV_TYPE_GENERIC;
}

MAV_MODE mavproxy_base_mode()
{
    Motors_HandleTypeDef *motors = fms.motors;

    uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
    // work out the base_mode. This value is not very useful
    // for APM, but we calculate it as best we can so a generic
    // MAVLink enabled ground station can work out something about
    // what the MAV is up to. The actual bit values are highly
    // ambiguous for most of the APM flight modes. In practice, you
    // only get useful information from the custom_mode, which maps to
    // the APM flight mode and has a well defined meaning in the
    // ArduPlane documentation
    switch (fms.flightmode->mode_number()) {
    case AUTO:
    case AUTO_RTL:
    case RTL:
    case LOITER:
    case AVOID_ADSB:
    case FOLLOW:
    case GUIDED:
    case CIRCLE:
    case POSHOLD:
    case BRAKE:
    case SMART_RTL:
        _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
        // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
        // APM does in any mode, as that is defined as "system finds its own goal
        // positions", which APM does not currently do
        break;
    default:
        break;
    }

    // all modes except INITIALISING have some form of manual
    // override if stick mixing is enabled
    _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;

    // we are armed if we are not initialising
    if (motors != NULL && motors->_armed) {
        _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
    }

    // indicate we have set a custom mode
    _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

    return (MAV_MODE)_base_mode;
}

MAV_STATE mavproxy_vehicle_system_status()
{
    // set system as critical if any failsafe have triggered
    if (fms.failsafe.radio || 
        fms.failsafe.gcs || 
        fms.failsafe.ekf || 
        fms.failsafe.terrain || 
        fms.failsafe.adsb)  {
        return MAV_STATE_CRITICAL;
    }

    if (fms.ap.land_complete) {
        return MAV_STATE_STANDBY;
    }

    return MAV_STATE_ACTIVE;
}

uint32_t mavproxy_custom_mode()
{
    return (uint32_t)fms.flightmode->mode_number();
}

void mavproxy_send_banner()
{
    // mark the firmware version in the tlog
    mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s", "grapilot V0.0.1-dev");

    mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s: %s",
                  "OS", "RT-Thread");

    // send system ID if we can
    //char sysid[40];
    //mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s", "");
}

/*
  code common to both SET_MODE mavlink message and command long set_mode msg
*/
MAV_RESULT mavproxy_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode)
{
    // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
    if ((uint32_t)_base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        if (!fms_set_mode(_custom_mode, MODE_REASON_GCS_COMMAND)) {
            // often we should be returning DENIED rather than FAILED
            // here.  Perhaps a "has_mode" callback on AP_::vehicle()
            // would do?
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;
    }

    if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
        // set the safety switch position. Must be in a command by itself
        if (_custom_mode == 0) {
            // turn safety off (pwm outputs flow to the motors)
            //hal.rcout->force_safety_off();
            return MAV_RESULT_ACCEPTED;
        }
        if (_custom_mode == 1) {
#if 0
            // turn safety on (no pwm outputs to the motors)
            if (hal.rcout->force_safety_on()) {
                return MAV_RESULT_ACCEPTED;
            }
            return MAV_RESULT_FAILED;
#endif
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_DENIED;
    }

    // Command is invalid (is supported but has invalid parameters)
    return MAV_RESULT_DENIED;
}

void mavproxy_handle_set_mode(mavlink_message_t *msg)
{
    mavlink_set_mode_t packet;
    mavlink_msg_set_mode_decode(msg, &packet);

    const MAV_MODE _base_mode = (MAV_MODE)packet.base_mode;
    const uint32_t _custom_mode = packet.custom_mode;

    const MAV_RESULT result = mavproxy_set_mode_common(_base_mode, _custom_mode);

    // send ACK or NAK.  Note that this is extraodinarily improper -
    // we are sending a command-ack for a message which is not a
    // command.  The command we are acking (ID=11) doesn't actually
    // exist, but if it did we'd probably be acking something
    // completely unrelated to setting modes.
    mavproxy_send_command_ack_sysid(MAVLINK_MSG_ID_SET_MODE, result, msg);
}

/*------------------------------------test------------------------------------*/


